#! /usr/bin/env python
'''
    该类为解锁机器人的类，和姿态解算层的接口进行通信
   该类的作用是用来解锁机器人，当从其他状态切换成该状态时，需要执行publish命令发布“on”字符串，将机器人解锁
   同时实时监听状态，如果需要退出则退出
'''
from threading import Thread
from time import sleep
import rospy
from std_msgs.msg import String

#自定义类
class OnCase(Thread):

 def __init__(self,queue):
    Thread.__init__(self)
    #获取话题名称  LockTopic: lock_cmd
    topicName = rospy.get_param("LockTopic","lock_cmd")
    #设置发布者publisher
    self.publisher = rospy.Publisher(topicName,String,queue_size=10)
    self.queue = queue
    self.cmd = None
    self.exit = False

 def execute(self):
      print("Unlock robot!")
      #发布解锁机器人的话题
      self.prelude()
      while not self.exit:
         #时刻检查更新
         self.update()
         sleep(0.1)
      self.epilogue()

 def run(self):
    self.execute()

 def update(self):
     while(not self.queue.empty()):
         self.cmd = self.queue.get()
         if(self.cmd == "quit"):
            self.exit = True

 def epilogue(self):
   #  super().terminate()
    print("Unlock process ending")

 def prelude(self):
    msg = String()
    msg.data = rospy.get_param("UnlockCmd","on")
    print("///////////////////////////////////////")
    print(msg)
    self.publisher.publish(msg)

